//
// Created by ultcrt on 2021/4/27.
//

#include "radar_driver.h"
#include <JetsonGPIO.h>
#include <chrono>
#include <thread>

using namespace std;
using namespace GPIO;

uint64_t time_ns() {
    return chrono::duration_cast<chrono::nanoseconds>(chrono::system_clock::now().time_since_epoch()).count();
}

RadarDriver::RadarDriver() {
    setmode(BOARD);
    setup(RX_A, IN);
    setup(TX_A, OUT, LOW);
    setup(RX_B, IN);
    setup(TX_B, OUT, LOW);
}

RadarDriver::~RadarDriver() {
    cleanup(RX_A);
    cleanup(TX_A);
    cleanup(RX_B);
    cleanup(TX_B);
}

double RadarDriver::get() {
    promise<double> radar_a_prom, radar_b_prom;
    future<double> radar_a_fur = radar_a_prom.get_future();
    future<double> radar_b_fur = radar_b_prom.get_future();
    thread radar_a(&RadarDriver::detect, this, RX_A, TX_A, ref(radar_a_prom));
    thread radar_b(&RadarDriver::detect, this, RX_B, TX_B, ref(radar_b_prom));

    // Will cause weird problem if not join or detach before future get
    radar_a.join();
    radar_b.join();

    return min(radar_a_fur.get(), radar_b_fur.get());
}

void RadarDriver::detect(int rx, int tx, promise<double> &ret) {
    // Trigger emission
    output(tx, HIGH);

    // trigger time
    uint64_t trigger_time = time_ns();

    // Continue trigger signal for more than 10ns
    while(time_ns() - trigger_time < 10);
    output(tx, LOW);

    // Wait for sent signal
    uint64_t timeout = time_ns();
    while (input(rx) != HIGH) {
        // Max distance 4m
        if (time_ns() - timeout >= 24705882) {
            ret.set_value(-1.00);
            return;
        }
    }
    uint64_t sent_time = time_ns();

    // Wait for received signal
    timeout = time_ns();
    while (input(rx) != LOW) {
        // Max distance 4m
        if (time_ns() - timeout >= 24705882) {
            ret.set_value(999.99);
            return;
        }
    }
    uint64_t received_time = time_ns();

    ret.set_value(double(received_time - sent_time) * SONIC_SPEED / 2);
}
